
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_compass_ist8310.c
  * @author     baiyang
  * @date       2022-6-6
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include "sensor_compass_ist8310.h"

#include <common/console/console.h>
#include <common/utility/sparse-endian.h>
/*-----------------------------------macro------------------------------------*/
#define WAI_REG 0x0
#define DEVICE_ID 0x10

#define OUTPUT_X_L_REG 0x3
#define OUTPUT_X_H_REG 0x4
#define OUTPUT_Y_L_REG 0x5
#define OUTPUT_Y_H_REG 0x6
#define OUTPUT_Z_L_REG 0x7
#define OUTPUT_Z_H_REG 0x8

#define CNTL1_REG 0xA
#define CNTL1_VAL_SINGLE_MEASUREMENT_MODE 0x1

#define CNTL2_REG 0xB
#define CNTL2_VAL_SRST 1

#define AVGCNTL_REG 0x41
#define AVGCNTL_VAL_XZ_0  (0)
#define AVGCNTL_VAL_XZ_2  (1)
#define AVGCNTL_VAL_XZ_4  (2)
#define AVGCNTL_VAL_XZ_8  (3)
#define AVGCNTL_VAL_XZ_16 (4)
#define AVGCNTL_VAL_Y_0  (0 << 3)
#define AVGCNTL_VAL_Y_2  (1 << 3)
#define AVGCNTL_VAL_Y_4  (2 << 3)
#define AVGCNTL_VAL_Y_8  (3 << 3)
#define AVGCNTL_VAL_Y_16 (4 << 3)

#define PDCNTL_REG 0x42
#define PDCNTL_VAL_PULSE_DURATION_NORMAL 0xC0

#define SAMPLING_PERIOD_USEC (10 * GP_USEC_PER_MSEC)

/*
 * FSR:
 *   x, y: +- 1600 µT
 *   z:    +- 2500 µT
 *
 * Resolution according to datasheet is 0.3µT/LSB
 */
#define IST8310_RESOLUTION 0.3
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static bool init(sensor_compass_ist8310_t ist8310);
static void start_conversion(sensor_compass_ist8310_t ist8310);
static void timer(void *parameter);
static void read(sensor_compass_backend *compass_backend);
/*----------------------------------variable----------------------------------*/
static const int16_t IST8310_MAX_VAL_XY = (1600 / IST8310_RESOLUTION) + 1;
static const int16_t IST8310_MIN_VAL_XY = -IST8310_MAX_VAL_XY;
static const int16_t IST8310_MAX_VAL_Z  = (2500 / IST8310_RESOLUTION) + 1;
static const int16_t IST8310_MIN_VAL_Z  = -IST8310_MAX_VAL_Z;

struct sensor_compass_backend_ops ist8310_ops;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void sensor_compass_ist8310_ctor(sensor_compass_ist8310_t ist8310, gp_device_t dev,
                                       bool force_external,
                                       enum RotationEnum rotation)
{
    // 清空sensor_imu_backend结构体变量，因为sensor_imu_backend结构体有可能是申请的动态内存
    // 防止sensor_imu_backend中的变量初始为非零值。
    rt_memset(ist8310, 0, sizeof(struct sensor_compass_ist8310));

    sensor_compass_backend_ctor(&ist8310->backend, "ist8310");

    ist8310_ops = compass_backend_ops;
    ist8310_ops.read = read;

    ist8310->backend.ops = &ist8310_ops;
    ist8310->_dev = dev;

    ist8310->_force_external = force_external;
    ist8310->_rotation       = rotation;
}

/**
* @brief       
* @param[in]   dev  
* @param[in]   force_external  
* @param[in]   rotation  
* @param[out]  
* @retval      
* @note        
*/
sensor_compass_backend *sensor_compass_ist8310_probe(gp_device_t dev, bool force_external, enum RotationEnum rotation)
{
    if (!dev) {
        return NULL;
    }

    sensor_compass_ist8310_t sensor = (sensor_compass_ist8310_t)rt_malloc(sizeof(struct sensor_compass_ist8310));

    if (!sensor) {
        return NULL;
    }

    sensor_compass_ist8310_ctor(sensor, dev, force_external, rotation);

    if (!init(sensor)) {
        sensor_compass_backend_destructor(&sensor->backend);
        rt_free(sensor);
        return NULL;
    }

    return (sensor_compass_backend *)sensor;
}

static bool init(sensor_compass_ist8310_t ist8310)
{
    uint8_t reset_count = 0;

    devmgr_take_bus(ist8310->_dev);

    // high retries for init
    devmgr_set_retries(ist8310->_dev, 10);

    uint8_t whoami;
    if (!devmgr_read_registers(ist8310->_dev, WAI_REG, &whoami, 1) ||
        whoami != DEVICE_ID) {
        // not an IST8310
        goto fail;
    }

    for (; reset_count < 5; reset_count++) {
        if (!devmgr_write_register(ist8310->_dev, CNTL2_REG, CNTL2_VAL_SRST, false)) {
            rt_thread_mdelay(10);
            continue;
        }

        rt_thread_mdelay(10);

        uint8_t cntl2 = 0xFF;
        if (devmgr_read_registers(ist8310->_dev, CNTL2_REG, &cntl2, 1) &&
            (cntl2 & 0x01) == 0) {
            break;
        }
    }

    if (reset_count == 5) {
        console_printf("IST8310: failed to reset device\n");
        goto fail;
    }

    if (!devmgr_write_register(ist8310->_dev, AVGCNTL_REG, AVGCNTL_VAL_Y_16 | AVGCNTL_VAL_XZ_16, false) ||
        !devmgr_write_register(ist8310->_dev, PDCNTL_REG, PDCNTL_VAL_PULSE_DURATION_NORMAL, false)) {
        console_printf("IST8310: found device but could not set it up\n");
        goto fail;
    }

    // lower retries for run
    devmgr_set_retries(ist8310->_dev, 3);

    // start state machine: request a sample
    start_conversion(ist8310);

    devmgr_release_bus(ist8310->_dev);

    // register compass instance
    devmgr_set_device_type(ist8310->_dev, COMPASS_DEVTYPE_IST8310);
    if (!sensor_compass_register_compass(devmgr_get_bus_id(ist8310->_dev), &ist8310->_instance)) {
        return false;
    }
    sensor_compass_backend_set_dev_id(ist8310->_instance, devmgr_get_bus_id(ist8310->_dev));

    console_printf("%s found on bus %u id %u address 0x%02x\n", "ist8310",
           ist8310->_dev->d.devid_s.bus, (unsigned)devmgr_get_bus_id(ist8310->_dev), ist8310->_dev->d.devid_s.address);

    sensor_compass_backend_set_rotation(ist8310->_instance, ist8310->_rotation);

    if (ist8310->_force_external) {
        sensor_compass_backend_set_external(ist8310->_instance, true);
    }
    
    ist8310->_periodic_handle = devmgr_register_periodic_callback(ist8310->_dev, SAMPLING_PERIOD_USEC,
        timer, ist8310);

    devmgr_release_bus(ist8310->_dev);
    return true;

fail:
    devmgr_release_bus(ist8310->_dev);
    return false;
}

static void start_conversion(sensor_compass_ist8310_t ist8310)
{
    if (!devmgr_write_register(ist8310->_dev, CNTL1_REG, CNTL1_VAL_SINGLE_MEASUREMENT_MODE, false)) {
        ist8310->_ignore_next_sample = true;
    }
}

static void timer(void *parameter)
{
    sensor_compass_ist8310_t ist8310 = (sensor_compass_ist8310_t)parameter;

    if (ist8310->_ignore_next_sample) {
        ist8310->_ignore_next_sample = false;
        start_conversion(ist8310);
        return;
    }

    // 定义一字节对齐的匿名结构体
    struct PACKED {
        le16_t rx;
        le16_t ry;
        le16_t rz;
    } buffer;

    bool ret = devmgr_read_registers(ist8310->_dev, OUTPUT_X_L_REG, (uint8_t *) &buffer, sizeof(buffer));
    if (!ret) {
        return;
    }

    start_conversion(ist8310);

    /* same period, but start counting from now */
    devmgr_adjust_periodic_callback(ist8310->_dev, ist8310->_periodic_handle, SAMPLING_PERIOD_USEC);

    int16_t x = (int16_t)(le16toh(buffer.rx));
    int16_t y = (int16_t)(le16toh(buffer.ry));
    int16_t z = (int16_t)(le16toh(buffer.rz));

    /*
     * Check if value makes sense according to the FSR and Resolution of
     * this sensor, discarding outliers
     */
    if (x > IST8310_MAX_VAL_XY || x < IST8310_MIN_VAL_XY ||
        y > IST8310_MAX_VAL_XY || y < IST8310_MIN_VAL_XY ||
        z > IST8310_MAX_VAL_Z  || z < IST8310_MIN_VAL_Z) {
        return;
    }

    // flip Z to conform to right-hand rule convention
    z = -z;

    /* Resolution: 0.3 µT/LSB - already convert to milligauss */
    Vector3f_t field = {x * 3.0f, y * 3.0f, z * 3.0f};

#ifdef HAL_IST8310_I2C_HEATER_OFFSET
    /*
      the internal IST8310 can be impacted by the magnetic field from
      a heater. We use the heater duty cycle to correct for the error
     */
    if (!is_external(_instance) && AP_HAL::Device::devid_get_bus_type(_dev->get_bus_id()) == AP_HAL::Device::BUS_TYPE_I2C) {
        const auto *bc = AP::boardConfig();
        if (bc) {
            field += HAL_IST8310_I2C_HEATER_OFFSET * bc->get_heater_duty_cycle() * 0.01;
        }
    }
#endif

    sensor_compass_backend_accumulate_sample(&ist8310->backend, &field, ist8310->_instance, 10);
}

static void read(sensor_compass_backend *compass_backend)
{
    sensor_compass_ist8310_t ist8310 = (sensor_compass_ist8310_t)compass_backend;

    sensor_compass_backend_drain_accumulated_samples(compass_backend, ist8310->_instance, NULL);
}

/*------------------------------------test------------------------------------*/


